﻿using System;
using System.IO;
using System.Windows;

namespace Util
{
    public class PIDTool
    {
        //调用PID算法
        public static void cal()
        {
            double target = 30;//目标位置
            int size = 30; //一共走30秒，每一秒采样一次

            double Kp = 0;//P的系数
            double Ki = 0;//I的系数
            double Kd = 0;//D的系数

            double max = 3; //PID三个系数的最大值
            double speed = 0;//速度
            double jump = 0.2;//PID三个系数的每次变化的幅度

            for (Kd = 0; Kd < max; Kd += jump)
            {
                for (Ki = 0; Ki < max; Ki += jump)
                {
                    for (Kp = 0.2; Kp < max; Kp += jump)
                    {
                        double[] arrPosition = new double[size];//保存数据的数组
                        double p1 = 0;//第一个位置
                        double p2 = 0;//第二个位置

                        //在确定的Kp, Ki, Kd的情况下，调用一次PID算法
                        for (int i = 0; i < size; i++)
                        {
                            speed = PIDTool.PID(p1, p2, target, 2, Kp, Ki, Kd);

                            p1 = p2; //更新
                            p2 += speed;

                            arrPosition[i] = p2;
                        }


                    }
                }
            }
        }

        //从0的位置前进到10的位置，每一秒检测一次。用PID算法计算速度。速度有正负号
        public static double PID(double p1, double p2, double target, double? MaxSeep, double Kp, double Ki, double Kd)
        {
            double err1 = target - p1; //上一个误差
            double err2 = target - p2; //当前的误差
            double err = err1 + err2; //总误差
            double derr = err2 - err1; //误差的导数

            //P
            double Up = Kp * err2;
            if (Up > 2)
            {
                Up = Convert.ToDouble(MaxSeep); //限速
            }

            //I
            double Ui = 0;

            Ui = Ki * err;
            if (Ui > 0.3)
            {
                Ui = 0.3;//限速
            }

            //D
            double Ud = 0;
            Ud = Kd * derr;




            if (Up + Ui - Ud > MaxSeep)
            {
                return Convert.ToDouble(MaxSeep); //限速
            }

            //综合PID
            return Up + Ui - Ud;
        }

        static bool Flag = true;

        public void WriteConfigToTxt(string strInfo)
        {

            if (Flag)
            {
                Flag = false;
                DateTime dt = DateTime.Now;
                try
                {
                    string configPath = "C:\\CCC";
                    //判断该路径下文件夹是否存在，不存在的情况下新建文件夹
                    if (!Directory.Exists(configPath))
                    {
                        Directory.CreateDirectory(configPath);
                    }
                    //指定日志文件的目录
                    string fname = configPath + "\\ABC.txt";



                    //定义文件信息对象
                    FileInfo finfo = new FileInfo(fname);

                    if (!finfo.Exists)
                    {
                        FileStream fs;
                        fs = File.Create(fname);
                        fs.Close();
                        finfo = new FileInfo(fname);
                    }

                    //创建只写文件流
                    using (FileStream fs = finfo.OpenWrite())
                    {
                        //根据上面创建的文件流创建写数据流
                        StreamWriter w = new StreamWriter(fs);

                        //设置写数据流的起始位置为文件流的末尾
                        w.BaseStream.Seek(0, SeekOrigin.End);

                        //写入内容
                        w.Write(dt.ToString("yyyy-MM-dd HH:mm:ss.fff") + "  " + strInfo + "\n\r");

                        //清空缓冲区内容，并把缓冲区内容写入基础流
                        w.Flush();

                        //关闭写数据流
                        w.Close();
                    }
                    Flag = true;
                }
                catch (Exception ex)
                {
                    MessageBox.Show(ex.Message, "日志写入出错");
                    Flag = true;
                    throw;
                }

            }
        }
    }
}
